﻿#pragma once

#include "low_outlier_filter.h"

#include <pcl/common/io.h>
#include <pcl/common/common.h>

#include <Eigen/Core>
using namespace pcl;
namespace oil
{

template < typename PointT >
void LowOutlierFilter<PointT>::applyFilter(PointCloud &output)
{
	std::vector<int> indices;
	if (keep_organized_)
	{
		bool temp = extract_removed_indices_;
		extract_removed_indices_ = true;
		applyFilterIndices(indices);
		extract_removed_indices_ = temp;

		output = *input_;
		for (int rii = 0; rii < static_cast<int> (removed_indices_->size()); ++rii)  // rii = removed indices iterator
			output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
		if (!pcl_isfinite(user_filter_value_))
			output.is_dense = false;
	}
	else
	{
		applyFilterIndices(indices);
		copyPointCloud(*input_, indices, output);
	}
}


template < typename PointT >
void LowOutlierFilter<PointT>::applyFilterIndices(std::vector<int> &indices)
{
	if (extract_removed_indices_)
	{
		removed_indices_.reset(new std::vector<int>);
		removed_indices_->reserve(1000);
	}

	double low_threshold = low_threshold_;
	double resolution = window_size_;
	int half_size = mophological_size_;
	// setup grid based on scale and extents
	Eigen::Vector4f global_max, global_min;
	pcl::getMinMax3D<PointT>(*input_, global_min, global_max);

	float xextent = global_max.x() - global_min.x();
	float yextent = global_max.y() - global_min.y();

	int rows = static_cast<int> (std::floor(yextent / resolution) + 1);
	int cols = static_cast<int> (std::floor(xextent / resolution) + 1);

	//A 记录DSM，这个DSM其实就是窗口内的最低点
	Eigen::MatrixXf A(rows, cols);
	A.setConstant(std::numeric_limits<float>::quiet_NaN());

	//Zf记录变换后的第二低点
	Eigen::MatrixXf Zf(rows, cols);
	Zf.setConstant(std::numeric_limits<float>::quiet_NaN());

	//Z 记录变换后的最低点
	Eigen::MatrixXf Z(rows, cols);
	Z.setConstant(std::numeric_limits<float>::quiet_NaN());

	//A 记录DSM，这个DSM其实就是窗口内的最低点
	for (int i = 0; i < (int)indices_->size(); ++i)
	{
		const PointT& p = input_->points[(*indices_)[i]];
		int row = static_cast<int> (std::floor((p.y - global_min.y()) / resolution));
		int col = static_cast<int> (std::floor((p.x - global_min.x()) / resolution));

		if (p.z < A(row, col) || pcl_isnan(A(row, col)))
		{
			A(row, col) = p.z;
		}
	}
	
	
	//计算Zf
	for (int row = 0; row < rows; ++row)
	{
		int rs, re;
		rs = ((row - half_size) < 0) ? 0 : row - half_size;
		re = ((row + half_size) > (rows - 1)) ? (rows - 1) : row + half_size;

		for (int col = 0; col < cols; ++col)
		{
			int cs, ce;
			cs = ((col - half_size) < 0) ? 0 : col - half_size;
			ce = ((col + half_size) > (cols - 1)) ? (cols - 1) : col + half_size;

			int num_grids = (ce - cs + 1) * (re - rs + 1);
			std::vector<float> grid_values(num_grids, std::numeric_limits<float>::max());

			int pos = 0;
			for (int j = rs; j < (re + 1); ++j)
			{
				for (int k = cs; k < (ce + 1); ++k)
				{
					if (!pcl_isnan(A(j, k)))
					{
						grid_values[pos++] = A(j, k);
					}
					else
					{
						++pos;
					}
				}
			}
			std::sort(begin(grid_values), end(grid_values));
			//假设最小的是low_outlier，这里只取到第n小的
			int median = num_grids / 2;
			float second_minimum = grid_values[median];
			if (A(row, col) < second_minimum)
			{
				Zf(row, col) = second_minimum;
			}
			else if(A(row, col) >= second_minimum)
			{
				Zf(row, col) = A(row, col);
			}

			//如果这个地方的A(row, col) 是NAN的话，此时Zf也会是NAN
			//所以在这里会做一个判断，如果是NAN的话，则把他赋值为second_minimum
			//这里second_minimum 很有可能是max()的值
			//没有关系，本来Zf就是为了做一个开运算，得到局部最大值
			//需要注意的是，这里second_minimum不可能是Nan，值可能是最大值
			if (pcl_isnan(Zf(row, col)))
			{
				Zf(row, col) = second_minimum;
			}
		}
	}

	//计算Z
	for (int row = 0; row < rows; ++row)
	{
		int rs, re;
		rs = ((row - half_size) < 0) ? 0 : row - half_size;
		re = ((row + half_size) > (rows - 1)) ? (rows - 1) : row + half_size;

		for (int col = 0; col < cols; ++col)
		{
			int cs, ce;
			cs = ((col - half_size) < 0) ? 0 : col - half_size;
			ce = ((col + half_size) > (cols - 1)) ? (cols - 1) : col + half_size;

			float min_coeff = std::numeric_limits<float>::max();

			for (int j = rs; j < (re + 1); ++j)
			{
				for (int k = cs; k < (ce + 1); ++k)
				{
					if (!pcl_isnan(Zf(j, k)))
					{
						if (Zf(j, k) < min_coeff)
							min_coeff = Zf(j, k);
					}
				}
			}
			//如果Zf都是Nan也会给一个值，这个值就是最大值
			//因此在这个旁边的点，都会认为是low outlier，但是其实不会出现这种情况
			//因为如果他旁边有点，Zf也不会是Nan
			Z(row, col) = min_coeff;
		}
	}

	//计算low outlier
	std::vector<int> pt_indices;
	pt_indices.reserve(1000);
	for (size_t p_idx = 0; p_idx < indices_->size(); ++p_idx)
	{
		const PointT& p = input_->points[(*indices_)[p_idx]];
		int erow = static_cast<int> (std::floor((p.y - global_min.y()) / resolution));
		int ecol = static_cast<int> (std::floor((p.x - global_min.x()) / resolution));

		float diff = p.z - Z(erow, ecol);

		//negetive=true的话，则得到low outlier
		if (negative_)
		{
			if (diff < -low_threshold)
			{
				pt_indices.push_back((*indices_)[p_idx]);
			}
			else if (extract_removed_indices_)
			{
				removed_indices_->push_back((*indices_)[p_idx]);
			}
		}
		//否则得到的是非outlier点
		else
		{
			if (diff >= -low_threshold)
			{
				pt_indices.push_back((*indices_)[p_idx]);
			}
			else if (extract_removed_indices_)
			{
				removed_indices_->push_back((*indices_)[p_idx]);
			}
		}		
	}
	indices = pt_indices;	
}



}
#define PCL_INSTANTIATE_LowOutlierFilter(T) template class PCL_EXPORTS oil::LowOutlierFilter<T>;
